from numpy.typing import NDArray


def get_min_score(queue,scoreMap):
  if len(queue)==0:
    return None
  return min(queue, key = lambda k: scoreMap[k])
  
# a*寻路算法
def aStar(map:NDArray,start,end,ignoneM=True):
  
  def get_between(pont1,pont2):
    return abs(pont1[0]-pont2[0])+abs(pont1[1]-pont2[1])
  
  def get_h_score(h_start):
    return get_between(h_start, end)
  
  def get_neighbor(point):
    x,y=point
    dir=[[-1,0],[0,1],[1,0],[0,-1]]
    nodes=[]
    for dx,dy in dir:
      nx=dx+x
      ny=dy+y
      if 0<=nx<map.shape[0] and 0<=ny<map.shape[1]:
        nodes.append((nx,ny))
    return nodes
  
  def reconstruct_path(came_from, current):
    total_path=[current]
    while current in came_from.keys():
      current=came_from[current]
      total_path.insert(0,current)
    return total_path
  # E 自定义障碍
  block=('FH',"CH","RH","PH","OH","R",'T',"W","E") 
  x,y=start
  j,k=end
  came_from={}
  closeSet=set()
  openSet=set()
  openSet.add(start)
  g_score={}
  g_score[(x, y)]=0
  h_score={}
  h_score[start]=get_h_score((x, y))
  f_score={}
  f_score[start]=h_score[start]
  
  
  while len(openSet)!=0:
    curNode=get_min_score(openSet,f_score)
    if curNode==(j,k):
      return reconstruct_path(came_from, curNode)
    openSet.remove(curNode)
    closeSet.add(curNode)
    for neighbor in get_neighbor(curNode):
      if neighbor in closeSet or map[curNode] in block:
        continue
      tentative_g_score=get_between(curNode, neighbor) +g_score[curNode]
      
      if (not ignoneM) and map[curNode]=='M' or map[neighbor] =='M':
        # 如果是泥地且不忽视,则要加权
        tentative_g_score+=1
        
      tentative_is_better=False
      if neighbor not in openSet:
        tentative_is_better=True
      elif tentative_g_score<g_score[neighbor]:
        tentative_is_better=True
      else:
        tentative_is_better=False
      if tentative_is_better:
        came_from[neighbor]=curNode
        g_score[neighbor]=tentative_g_score
        h_score[neighbor]=get_h_score(neighbor)
        f_score[neighbor]=g_score[neighbor]+h_score[neighbor]
        openSet.add(neighbor)
        

  return None
